#ifndef UTILS_H
#define UTILS_H

#include <Eigen/Dense>
#include <opencv2/core/core.hpp>

template<typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> cvMatToEigen(const cv::Mat& cvMat) {
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> eigenMat;
    eigenMat.resize(cvMat.rows, cvMat.cols);
    
    for (int i = 0; i < cvMat.rows; ++i) {
        for (int j = 0; j < cvMat.cols; ++j) {
            eigenMat(i, j) = cvMat.at<T>(i, j);
        }
    }
    
    return eigenMat;
}

// 生成测试数据
void GenerateTestData(std::vector<Eigen::Vector3d>& landmarks,
                     std::vector<Eigen::Vector2d>& bearings,
                     Eigen::Vector3d& true_translation,
                     Eigen::Quaterniond& true_rotation,
                     double fx = 520.0, double fy = 520.0, 
                     double cx = 325.0, double cy = 245.0) {
    
    landmarks.clear();
    bearings.clear();
    
    // 生成一些3D点
    for (int i = 0; i < 50; ++i) {
        Eigen::Vector3d pt;
        pt << (i - 25) * 0.5, (i % 10 - 5) * 0.3, 5.0 + (i % 5) * 0.2;
        landmarks.push_back(pt);
    }
    
    // 真实位姿
    true_rotation = Eigen::Quaterniond(0.9239, 0.2, 0.1, 0.3).normalized(); // 大约30度旋转
    true_translation << 0.1, -0.2, 0.3;
    
    // 生成2D观测
    for (const auto& pt : landmarks) {
        Eigen::Vector3d p_c = true_rotation * pt + true_translation;
        double u = fx * (p_c[0] / p_c[2]) + cx;
        double v = fy * (p_c[1] / p_c[2]) + cy;
        bearings.push_back(Eigen::Vector2d(u, v));
    }
    
    std::cout << "Generated " << landmarks.size() << " test points" << std::endl;
}

#endif // UTILS_H